
#!/usr/bin/python
import sys
import _thread
import time
from ctypes import *
from MiniBotAlgorithmLib import MiniBotAlgorithmLib

# 实例化动态库接口类
mlib = MiniBotAlgorithmLib()

#setPositionCallback_type = CFUNCTYPE(None,c_byte, c_int )
#setPositionWithSpeedCallback_type = CFUNCTYPE(None,c_byte, c_int,c_int )
#setMultiPositionCallback_type = CFUNCTYPE(None,POINTER(c_byte), POINTER(c_double),c_int )
#setMultiPositionWithSpeedCallback_type = CFUNCTYPE(None,POINTER(c_byte), POINTER(c_double),c_int , c_int )
#setPositionSpeedCallback_type = CFUNCTYPE(None,c_byte, c_int )
#setRunningSpeedCallback_type = CFUNCTYPE(None,c_byte, c_int )
#setTorqueEnableCallback_type = CFUNCTYPE(None,c_byte, c_bool )
#setModeCallback_type = CFUNCTYPE(None,c_byte, c_int )


# ctypes通过 CFUNCTYPE 支持回调函数，定义返回值与参数，第一个参数表示返回值，void为None，第二参数为回调函数的参数为结构体指针
#def cal(a,b):
#   r=a+b
#   return r

#CALLBACK = CFUNCTYPE(c_int, c_int,c_int)
#callback=CALLBACK(cal)
#rs=mlib.lib.computer(c_int(1),c_int(2),callback)
#print("computer:"+str(rs))

def setPosition(id,pos):
 print("setPosition:"+str(id)+"---"+str(pos))

def setPositionWithSpeed(id,pos,speed):
 print("setPositionWithSpeed:"+str(id)+"---"+str(pos)+"---"+str(speed))

def setMultiPosition(id,pos,count):
 ids=[]
 poss=[]
 for i in range(count):  
   ids.append(id[i])
   poss.append(pos[i])
 #print("setMultiPosition:"+str(ids)+"---"+str(poss))

def setMultiPositionWithSpeed(id,pos,count,speed):
 print("setMultiPositionWithSpeed:"+str(id)+"---"+str(pos)+"---"+str(speed))

def setPositionSpeed(id,speed):
 print("setPositionSpeed:"+str(id)+"---"+str(speed))

def setRunningSpeed(id,speed):
 print("setRunningSpeed:"+str(id)+"---"+str(speed))

def setTorqueEnable(id,pos):
 print("setTorqueEnable:"+str(id)+"---"+str(pos))

def setMode(id,pos):
 print("setMode:"+str(id)+"---"+str(pos))

def runSanke():
 mlib.SendCmd(id,0, 0)

#回调函数类型声明
setPositionCallback_type = CFUNCTYPE(None,c_ubyte, c_double)
setPositionWithSpeedCallback_type = CFUNCTYPE(None,c_ubyte, c_double,c_int)
setMultiPositionCallback_type = CFUNCTYPE(None,POINTER(c_ubyte), POINTER(c_double),c_int)
setMultiPositionWithSpeedCallback_type = CFUNCTYPE(None,POINTER(c_ubyte), POINTER(c_double),c_int , c_int)
setSpeedCallback_type = CFUNCTYPE(None,c_ubyte, c_int)
setTorqueEnableCallback_type = CFUNCTYPE(None,c_ubyte, c_bool)
setModeCallback_type = CFUNCTYPE(None,c_ubyte, c_int)

#回调函数
setPositionCallback = setPositionCallback_type(setPosition)
setPositionWithSpeedCallback = setPositionWithSpeedCallback_type(setPositionWithSpeed)
setMultiPositionCallback = setMultiPositionCallback_type(setMultiPosition)
setMultiPositionWithSpeedCallback = setMultiPositionWithSpeedCallback_type(setMultiPositionWithSpeed)
setPositionSpeedCallback = setSpeedCallback_type(setPositionSpeed)
setRunningSpeedCallback = setSpeedCallback_type(setRunningSpeed)
setTorqueEnableCallback = setTorqueEnableCallback_type(setTorqueEnable)
setModeCallback = setModeCallback_type(setMode)

#新增机器人
id=mlib.AddRobot(2,[31,32,33,34,35])

#绑定关节处理函数
mlib.BindRobotModuleApis(id,setPositionCallback,setPositionWithSpeedCallback,setMultiPositionCallback,setMultiPositionWithSpeedCallback,setPositionSpeedCallback,setRunningSpeedCallback,setTorqueEnableCallback,setModeCallback)

_thread.start_new_thread(runSanke)
while(1):
    str = input("请输入命令（参考接口文档）：")
    cmd=int(str)
    #发送机器人控制命令
    mlib.SendCmd(id,cmd, 0);
    if(cmd==100):
     
     #停止机器人
     mlib.SendCmd(id,3, 0);
     #移除机器人
     mlib.RemoveRobot(id)
     break

